#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "ethercat.h"
#include <stdio.h>
#include <time.h>


#ifdef _WIN32
#include <windows.h>
#include <stdint.h>


#define CLOCK_MONOTONIC 0

#define SLAVE_POSITION 2  // Position of the slave
#define TARGET_POSITION 1000000  // Target position
#define PROFILE_VELOCITY 10000  // Profile velocity
#define PROFILE_ACCELERATION 500  // Profile acceleration
#define PROFILE_DECELERATION 500  // Profile deceleration


#define NSEC_PER_SEC 1000000000
#define EC_TIMEOUTMON 500
#define TIMER_ABSTIME   1

// struct sched_param schedp;
char IOmap[4096];
// pthread_t thread1, thread2;
uint64_t diff, maxt, avg, cycle;
struct timeval t1, t2;
int dorun = 0;
int64 toff = 0;
int expectedWKC;
boolean needlf;
volatile int wkc;
boolean inOP;
uint8 currentgroup = 0;


enum {
    STATE_RESET,
    STATE_INIT,
    STATE_PREREADY,
    STATE_READY,
    STATE_ENABLE,
    STATE_DISABLE
 };
 
 int state = STATE_RESET;
 
 typedef struct PACKED
 {
    uint16_t Controlword;
    int16_t TargetTor;
    int32_t TargetPos;
    int32_t TargetVel;
    uint8_t ModeOp;
    int32_t PPVel;
 } Drive_Outputs;
 
 typedef struct PACKED
 {
    uint16_t Statusword;
    int32_t ActualPos;
    int32_t ActualVel;
    int16_t ActualTor;
    uint8_t ModeOp;
    
 } Drive_Inputs;



int clock_gettime(int clk_id, struct timespec *tp) {
    if (clk_id != CLOCK_MONOTONIC) {
        return -1; 
    }

    LARGE_INTEGER frequency;
    LARGE_INTEGER time;

    if (!QueryPerformanceFrequency(&frequency)) {
        return -1;
    }

    if (!QueryPerformanceCounter(&time)) {
        return -1;
    }

    tp->tv_sec = (long)(time.QuadPart / frequency.QuadPart);
    tp->tv_nsec = (long)((time.QuadPart % frequency.QuadPart) * 1e9 / frequency.QuadPart);

    return 0;
}

// int clock_nanosleep(int clock_id, int flags, const timespec *request, timespec *remain) {
//     if (clock_id != CLOCK_MONOTONIC || (flags != 0 && flags != TIMER_ABSTIME)) {
//         errno = EINVAL; 
//         return -1;
//     }

//     if (request == NULL || request->tv_sec < 0 || request->tv_nsec < 0 || request->tv_nsec >= 1000000000) {
//         errno = EINVAL; 
//         return -1;
//     }

//     LARGE_INTEGER frequency, start, target, current;
//     QueryPerformanceFrequency(&frequency); 
//     QueryPerformanceCounter(&start);     

    
//     long long totalNanoseconds = ((long long)request->tv_sec * 1000000000LL) + request->tv_nsec;
//     long long targetTicks = (totalNanoseconds * frequency.QuadPart) / 1000000000LL;

//     if (flags == TIMER_ABSTIME) {
        
//         LARGE_INTEGER absoluteTime;
//         QueryPerformanceCounter(&absoluteTime);
//         long long currentTimeTicks = absoluteTime.QuadPart;

//         target.QuadPart = (targetTicks > currentTimeTicks) ? (currentTimeTicks + targetTicks) : currentTimeTicks;
//     } else {
        
//         target.QuadPart = start.QuadPart + targetTicks;
//     }


//     do {
//         QueryPerformanceCounter(&current);
//     } while (current.QuadPart < target.QuadPart);

//     return 0; 
// }


#endif



#ifdef _WIN32
#include <windows.h>  // Use Sleep function for Windows system
#else
#include <unistd.h>   // Use usleep function for Linux system
#endif




// Delay function
void delay_ms(int milliseconds) {
#ifdef _WIN32
    Sleep(milliseconds);  // Use Sleep for Windows, unit is milliseconds
#else
    usleep(milliseconds * 1000);  // Use usleep for Linux, unit is microseconds
#endif
}




 void add_timespec(struct timespec *ts, int64 addtime)
{
   long  nsec;
   time_t sec;

   nsec = addtime % NSEC_PER_SEC;
   sec = (addtime - nsec) / NSEC_PER_SEC;
   ts->tv_sec += sec;
   ts->tv_nsec += nsec;
   if (ts->tv_nsec > NSEC_PER_SEC)
   {
      nsec = ts->tv_nsec % NSEC_PER_SEC;
      ts->tv_sec += (ts->tv_nsec - nsec) / NSEC_PER_SEC;
      ts->tv_nsec = nsec;
   }
}

void ec_sync(int64 reftime, int64 cycletime, int64 *offsettime)
{
   static int64 integral = 0;
   int64 delta;
   /* set linux sync point 50us later than DC sync, just as example */
   delta = (reftime - 50000) % cycletime;
   if (delta > (cycletime / 2))
   {
      delta = delta - cycletime;
   }
   if (delta > 0)
   {
      integral++;
   }
   if (delta < 0)
   {
      integral--;
   }
   *offsettime = -(delta / 100) - (integral / 20);
}


 static int drive_write8(uint16 slave, uint16 index, uint8 subindex, uint8 value)
 {
    int wkc;
 
    wkc = ec_SDOwrite(slave, index, subindex, FALSE, sizeof(value), &value, EC_TIMEOUTRXM);
 
    return wkc;
 }
 
 static int drive_write16(uint16 slave, uint16 index, uint8 subindex, uint16 value)
 {
    int wkc;
 
    wkc = ec_SDOwrite(slave, index, subindex, FALSE, sizeof(value), &value, EC_TIMEOUTRXM);
 
    return wkc;
 }
 
 static int drive_write32(uint16 slave, uint16 index, uint8 subindex, int32 value)
 {
    int wkc;
 
    wkc = ec_SDOwrite(slave, index, subindex, FALSE, sizeof(value), &value, EC_TIMEOUTRXM);
 
    return wkc;
 }


int drive_setup(uint16 slave)
{
   int wkc = 0;

   printf("Drive setup\n");

   wkc += drive_write16(slave, 0x1C12, 0, 0);
   wkc += drive_write16(slave, 0x1C13, 0, 0);

   wkc += drive_write8(slave, 0x1A00, 0, 0);
   wkc += drive_write32(slave, 0x1A00, 1, 0x60410010); // Statusword
   wkc += drive_write32(slave, 0x1A00, 2, 0x60640020); // Position actual value
   wkc += drive_write32(slave, 0x1A00, 3, 0x606C0020); // Velocity actual value
   wkc += drive_write32(slave, 0x1A00, 4, 0x60770010); // Torque actual value
   wkc += drive_write32(slave, 0x1A00, 5, 0x60610008); // Modes of operation display
   wkc += drive_write8(slave, 0x1A00, 0, 5);

   wkc += drive_write8(slave, 0x1600, 0, 0);
   wkc += drive_write32(slave, 0x1600, 1, 0x60400010); // Controlword
   wkc += drive_write32(slave, 0x1600, 2, 0x60710010); // Target torque
   wkc += drive_write32(slave, 0x1600, 3, 0x607A0020); // Target position
   wkc += drive_write32(slave, 0x1600, 4, 0x60FF0020); // Target velocity
   wkc += drive_write32(slave, 0x1600, 5, 0x60600008); // Modes of operation display
   wkc += drive_write32(slave, 0x1600, 6, 0x60810020); // PP mode, velocity
   wkc += drive_write8(slave, 0x1600, 0, 6);

   wkc += drive_write16(slave, 0x1C12, 1, 0x1600);
   wkc += drive_write8(slave, 0x1C12, 0, 1);

   wkc += drive_write16(slave, 0x1C13, 1, 0x1A00);
   wkc += drive_write8(slave, 0x1C13, 0, 1);

   strncpy(ec_slave[slave].name, "Drive", EC_MAXNAME);

   if (wkc != 20)
   {
      printf("Drive %d setup failed\nwkc: %d\n", slave, wkc);
      return -1;
   }
   else
      printf("Drive %d setup succeed.\n", slave);

   return 0;
}


int SV630N(char* ifname)
{
    // Initialize EtherCAT master
    if (ec_init(ifname)) {
        printf("EtherCAT initialization succeeded.\n");
    }
    else {
        printf("EtherCAT initialization failed.\n");
        return -1;
    }

    // Configure slaves
    if (ec_config_init(FALSE) > 0) {
        printf("EtherCAT configuration succeeded.\n");
    }
    else {
        printf("EtherCAT configuration failed.\n");
        ec_close();
        return -1;
    }

    // Drive_Inputs *iptr;
    Drive_Outputs *optr;
    // struct sched_param schedp;
    // cpu_set_t mask;
    // pthread_t thread;
    int ht;
    int chk = 2000;
    int64 cycletime;
    struct timespec ts;


    int slave_ix;
    for (slave_ix = 2; slave_ix <= 3; slave_ix++)
    {
       ec_slavet *slave = &ec_slave[slave_ix];
       slave->PO2SOconfig = drive_setup;
    }

    /* configure DC options for every DC capable slave found in the list */
    ec_config_map(&IOmap); 
    ec_configdc(); 

    // setup dc for devices
    for (slave_ix = 2; slave_ix <= 3; slave_ix++)
    {
       ec_dcsync0(slave_ix, TRUE, 50000000U, 20000U);
       // ec_dcsync01(slave_ix, TRUE, 4000000U, 8000000U, 20000U);
    }

    printf("Slaves mapped, state to SAFE_OP.\n");
    ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE);

    /* read indevidual slave state and store in ec_slave[] */
    ec_readstate();
    for (slave_ix = 2; slave_ix <= 3; slave_ix++)
    {
       printf("Slave:%d Name:%s Output size:%3dbits Input size:%3dbits State:%2d delay:%d.%d\n",
        slave_ix, ec_slave[slave_ix].name, ec_slave[slave_ix].Obits, ec_slave[slave_ix].Ibits,
              ec_slave[slave_ix].state, (int)ec_slave[slave_ix].pdelay, ec_slave[slave_ix].hasdc);
    }
    expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
    printf("Calculated workcounter %d\n", expectedWKC);

    printf("Request operational state for all slaves\n");


    // Wait for all slaves to enter OP state
    ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE);

    printf("Request operational state for all slaves\n");
    /* activate cyclic process data */
    /* wait for all slaves to reach OP state */
    ec_slave[0].state = EC_STATE_OPERATIONAL;
    /* request OP state for all slaves */
    ec_writestate(0);

    clock_gettime(CLOCK_MONOTONIC, &ts);
    ht = (ts.tv_nsec / 1000000) + 1; /* round to nearest ms */
    ts.tv_nsec = ht * 1000000;
    cycletime = 50000 * 1000; /* cycletime in ns */

    for (int i = 2; i < 3; i++)
    {
       optr = (Drive_Outputs *)ec_slave[i + 1].outputs;
       if(optr == NULL)
       {
          printf("optr is NULL.\n");
       }
       optr->Controlword = 0;
       optr->TargetPos = 0;
       optr->ModeOp = 0;
       optr->TargetTor = 0;
       optr->TargetVel = 0;
       optr->PPVel = 0;
    }

    do
    {
       /* wait to cycle start */
       delay_ms(50);
       if (ec_slave[0].hasdc)
       {
          /* calulate toff to get linux time and DC synced */
          ec_sync(ec_DCtime, cycletime, &toff);
       }
       wkc = ec_receive_processdata(EC_TIMEOUTRET);
       ec_send_processdata();
    //    add_timespec(&ts, cycletime + toff);
    } while (chk-- && (wkc != expectedWKC)); 

    
    if (wkc == expectedWKC)
    {
       printf("Operational state reached for all slaves.\n");
    }
    else /* ECAT进入OP失败 */
    {
       printf("Not all slaves reached operational state.\n");
       ec_readstate();
       for (int i = 2; i <= 3; i++)
       {
          if (ec_slave[i].state != EC_STATE_OPERATIONAL)
          {
             printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n",
                    i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
          }
       }
    }


   //  ec_close();
    return 0;
}

char ifbuf[1024];
boolean printSDO = FALSE;
boolean printMAP = FALSE;

int main(int argc, char* argv[])
{
    ec_adaptert* adapter = NULL;
    printf("SOEM (Simple Open EtherCAT Master)\nSlaveinfo\n");

    if (argc > 1)
    {
        if ((argc > 2) && (strncmp(argv[2], "-sdo", sizeof("-sdo")) == 0)) printSDO = TRUE;
        if ((argc > 2) && (strncmp(argv[2], "-map", sizeof("-map")) == 0)) printMAP = TRUE;
        /* start slaveinfo */
        strcpy(ifbuf, argv[1]);
        SV630N(ifbuf);
    }
    else
    {
        printf("Usage: slaveinfo ifname [options]\nifname = eth0 for example\nOptions :\n -sdo : print SDO info\n -map : print mapping\n");
        /* Print the list */
        printf("Available adapters\n");
        adapter = ec_find_adapters();
        while (adapter != NULL)
        {
            printf("Description : %s, Device to use for wpcap: %s\n", adapter->desc, adapter->name);
            adapter = adapter->next;
        }
    }

    printf("End program\n");
    return (0);
}